﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace NIP.VisionProcessor
{
    public class VisionProcessor
    {
        public double FOVx { set; get; }
        public double FOVy { set; get; }
        public int RESx { set; get; }
        public int RESy { set; get; }
        public double d {set;get;}
        public double dInv {set;get;}
        public double[,] Px;
        public double[,] Py;
        public double[,] T;

        public VisionProcessor(double fx, double fy, int rx, int ry)
        {
            FOVx = fx;
            FOVy = fy;
            RESx = rx;
            RESy = ry;
            T = new double[4,4];
            Px = new double[RESx,RESy];
            Py = new double[RESx,RESy];
            d = RESx/(2.0*Math.Tan((FOVx*Math.PI/180.0)/2.0));
            dInv = 1.0/d;
            double xon2 = (RESx-1)/2.0;
            double yon2 = (RESy-1)/2.0;
            for(int x = 0; x < RESx; x++)
            {
                for(int y = 0; y < RESy; y++)
                {
                    Px[x,y] = (x - xon2)*dInv;
                    Py[x,y] = (y - yon2)*dInv;
                }
            }
        }

        //for a varying transformation matric ie: the wrist camera
        public double[] solveXYZ(int axis, double axisVal, int px, int py, double[,] t)
        {
            double[] returnVec = new double[3];
            setTmatrix(t);
            returnVec = getPixelVector(px,py,T);
            double D = (axisVal - T[axis, 3]) / returnVec[axis];
            returnVec[axis] = axisVal;
            for (int a = 0; a < 3; a++)
            {
                if (a != axis)
                {
                    returnVec[axis] *= D;
                    returnVec[axis] += T[axis, 3];
                }
            }
            return returnVec;
        }

        //for a constant transformation matric ie: the kinect
        public double[] solveXYZ(int axis, double axisVal, int px, int py)
        {
            double[] returnVec = new double[3];
            returnVec = getPixelVector(px, py, T);
            double D = (axisVal - T[axis, 3]) / returnVec[axis];
            returnVec[axis] = axisVal;
            for (int a = 0; a < 3; a++)
            {
                if (a != axis)
                {
                    returnVec[axis] *= D;
                    returnVec[axis] += T[axis, 3];
                }
            }
            return returnVec;
        }

        //should be used to set a constant transformation matrix, such as the kinect
        public void setTmatrix(double[,] t)
        {
            T = t;
        }

        private double[] getPixelVector(int px, int py, double[,] t)
        {
            double[] returnVec = new double[3];
            for(int i = 0; i < 3; i++)
            {
                returnVec[i] = Px[px,py]*t[i,0] - Py[px,py]*t[i,1] + t[i,2];
            }
            return returnVec;
        }



    }
}
